#include "SwarmAlgorithm.hpp"

LED* SwarmAlgorithm::red,
   * SwarmAlgorithm::green,
   * SwarmAlgorithm::blue;
        
Motor* SwarmAlgorithm::motorControl;
    
Bumper* SwarmAlgorithm::bumperControl;
    
Finder * SwarmAlgorithm::robotFinder;

vector<vector<string> > * SwarmAlgorithm::netMessages;

void SwarmAlgorithm::loadControls(LED *r, LED *g, LED *b, Motor *mot, Bumper *bump, Finder *finder, vector<vector<string> > *ms) {
    red = r;
    green = g;
    blue = b;
    motorControl = mot;
    bumperControl = bump;
    robotFinder = finder;
    netMessages = ms;
}

SwarmAlgorithm::SwarmAlgorithm() : isRunning(false) { }

string SwarmAlgorithm::getID() {
    return id;
}

LED* SwarmAlgorithm::getRed() {
    return SwarmAlgorithm::red;
}
LED* SwarmAlgorithm::getGreen() {
    return SwarmAlgorithm::green;
}
LED* SwarmAlgorithm::getBlue() {
    return SwarmAlgorithm::blue;
}

Motor* SwarmAlgorithm::getMotorControl() {
    return SwarmAlgorithm::motorControl;
}

Bumper* SwarmAlgorithm::getBumperControl() {
    return SwarmAlgorithm::bumperControl;
}

Finder * SwarmAlgorithm::getRobotFinder() {
    return SwarmAlgorithm::robotFinder;
}

bool SwarmAlgorithm::getIsRunning() {
    return isRunning;
}

vector<vector<string> > SwarmAlgorithm::readMessages(){
    vector<vector<string> > messages = *netMessages;
    netMessages->clear();
    return messages;	
}

string SwarmAlgorithm::readMessageStr() {
    while (netMessages->empty()) {
        SwarmOS::refreshNetworkMessages();
    }
    string retVal = netMessages->front()[0];
    netMessages->erase(netMessages->begin());
    return retVal;
}

void SwarmAlgorithm::turnLEDsOff() {
    red->setLED(false);
    green->setLED(false);
    blue->setLED(false);
}

// MAY RETURN NULL -- must have exacltly one LED on for its value
LED *SwarmAlgorithm::getLED() {
    LED *led = NULL;
    if (red->getIsOn()) {
        led = red;
    }
    if (blue->getIsOn()) {
        if (led == NULL) {
            led = blue;
        }
        else {
            led = NULL;
        }
    }
    if (green->getIsOn()) {
        if (led == NULL) {
            led = green;
        }
        else {
            led = NULL;
        }
    }
    return led;
}

// MAY RETURN NULL -- color must be a valid LED color
LED *SwarmAlgorithm::getLED(int color) {
    LED *led = NULL;
    switch (color) {
        case LED::RED:
            led = red;
            break;
        case LED::BLUE:
            led = blue;
            break;
        case LED::GREEN:
            led = green;
            break;
    }
    return led;
}

// MAY RETURN NULL -- color must be a valid LED color string
LED *SwarmAlgorithm::getLED(string colorStr) {
    int color = LED::colorStringToInt(colorStr);
    return getLED(color);
}

// returns index into param robots for nearest robot of specified color 
// (including LED::ANY), or -1 if no such robots found
int SwarmAlgorithm::getClosestRobotIndex(vector<Robot> robots, int color, bool forward)
{
    double radius = DBL_MAX;
    int robotId = -1;
    string colorStr = LED::intToColorString(color);
    for(int i = 0; i < robots.size(); i++)
    {
        Robot r = robots[i];
        double tempRad = r.position.radius_cm;
        double tempAng = fabs(r.position.angle_rad);
        if(tempRad < radius && (!forward || tempAng < M_PI_2) && (colorStr == LED::ANY_STR || colorStr == r.label))
        {
            robotId = i;
            radius = tempRad;
        }
    }
    return robotId;    
}

// returns index into param robots for farthest robot of specified color 
// (including LED::ANY), or -1 if no such robots found
int SwarmAlgorithm::getFurthestRobotIndex(vector<Robot> robots, int color)
{
    double radius = -1;
    int robotId = -1;
    string colorStr = LED::intToColorString(color);
    for(int i = 0; i < robots.size(); i++)
    {
        Robot r = robots[i];
        double tempRad = r.position.radius_cm;
        if(tempRad > radius && (colorStr == LED::ANY_STR || colorStr == r.label))
        {
            robotId = i;
            radius = tempRad;
        }
    }
    return robotId;    
}

// returns index into param robots for robot of specified color 
// (including LED::ANY), or -1 if no such robots found
int SwarmAlgorithm::getBumperRobotIndex(vector<Robot> robots, bool isFront, double offset, double furthestTouch)
{
    int robotId = -1;
    for(int i = 0; i < robots.size(); i++)
    {
        Robot r = robots[i];
        double tempRad = r.position.radius_cm;
        double tempAng = fabs(r.position.angle_rad);
        if(tempRad < furthestTouch && (isFront ? tempAng < offset : tempAng > offset))
        {
            robotId = i;
            furthestTouch = tempRad;
        }
    }
    return robotId;
}

unsigned char SwarmAlgorithm::getRandomMotorAction() {
    return rand() % 3 << MOTOR_LEFT | rand() % 3 << MOTOR_RIGHT;
}

void SwarmAlgorithm::performAction(unsigned char action, unsigned char rightOffset, unsigned char leftOffset) {
    // LED action
    if (~action & LED_SETN) {
        red  ->setLED(action & LED_ON << LED::RED);
        green->setLED(action & LED_ON << LED::GREEN);
        blue ->setLED(action & LED_ON << LED::BLUE);
    }

    // motor action

    // right
    switch (action & (MOTOR_BITS << MOTOR_RIGHT)) {
        case MOTOR_STOP:
            motorControl->rightWheelStop();
            break;
        case MOTOR_FORWARD << MOTOR_RIGHT:
            motorControl->rightWheelGoOffset(rightOffset);
            break;
        case MOTOR_REVERSE << MOTOR_RIGHT:
            motorControl->rightWheelReverseOffset(rightOffset);
            break;
    }

    // left
    switch (action & (MOTOR_BITS << MOTOR_LEFT)) {
        case MOTOR_STOP:
            motorControl->leftWheelStop();
            break;
        case MOTOR_FORWARD << MOTOR_LEFT:
            motorControl->leftWheelGoOffset(leftOffset);
            break;
        case MOTOR_REVERSE << MOTOR_LEFT:
            motorControl->leftWheelReverseOffset(leftOffset);
            break;
    }

    struct timespec tim, tim2;
    tim.tv_sec = 0;
    tim.tv_nsec = 500000000;
    nanosleep(&tim , &tim2);

    motorControl->stop();
}
